#pragma config(Sensor, in1,    pot,                 sensorPotentiometer)
#pragma config(Sensor, in7,    JumperTwo,           sensorPotentiometer)
#pragma config(Sensor, in8,    JumperOne,           sensorPotentiometer)
#pragma config(Sensor, dgtl1,  lshaft,              sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  ultra,               sensorSONAR_inch)
#pragma config(Sensor, dgtl11, rshaft,              sensorQuadEncoder)
#pragma config(Motor,  port1,           leftArm1,      tmotorNormal, openLoop)
#pragma config(Motor,  port3,           leftArm2,      tmotorNormal, openLoop)
#pragma config(Motor,  port4,           intake1,       tmotorNormal, openLoop)
#pragma config(Motor,  port5,           leftDB,        tmotorNormal, openLoop)
#pragma config(Motor,  port6,           rightDB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           intake2,       tmotorNormal, openLoop)
#pragma config(Motor,  port8,           rightArm1,     tmotorNormal, openLoop)
#pragma config(Motor,  port10,          rightArm2,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//DO NOT MODIFY ABOVE!!! (Configuration preprocessor directives)

/*
 * Team D VEX main and driver control competition code.
 * @author Derek Leung
 */

//Required code - do not modify!!
#pragma platform(VEX)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"

//Debugger disables arm power from regulate_arm
bool debug = false;

//Arm constants
const int ARM_BOTTOM = 875;
const int ARM_MAXERR = 5;
const int ARM_30 = 2680; //for scoring on 30" goals
const int ARM_20 = 1980; //for scoring on 20" goals
const int ARM_10 = 1340; //for intaking stack of two objects-robot skills
const int ARM_6 = 1120; //for intaking single object
const float ARM_PMULT = 0.25; //multiplier for P in PID
const float ARM_DMULT = 1.3; //multiplier for D in PID
const float ARM_IMAX = 300; //max value for I in PID
const float ARM_IMULT = 40/ARM_IMAX; //multiplier for I in PID (max power/ARM_IMAX)
const int ULTRA_MAX = 2;

//Arm variables
int real_pot;
float arm_err;
float arm_errprev=0;
float arm_errtot=0;
int arm_power;
int arm_ppower;
int arm_ipower;
int arm_dpower;
bool arm_move=false;
bool intake_move=false;
bool auton=false;

//Drive variables
int drive_target;
int lturn;
int rturn;
int pturn;
int left;
int right;
int correction;
int drive_distance;
int drive_err;
int drive_errprev;
int drive_ppower;
int drive_dpower;
int drive_power;

//Control constants
const int THRESHOLD = 25;       //minimum motor configuration to overcome static friction
const int SPLIT_POINT = 80;     //cannot equal 0!
const int DISCRETE_MAPPINGS = 4;
const int BOTTOM_SAFE_L = -10;
const int TOP_SAFE_L = 10;
const int BOTTOM_SAFE_R = -10;
const int TOP_SAFE_R = 10;

void pre_auton() {}
#include "d254x.c" //Autonomous functions


//////////////////////////////////
/////// Autonomous Control ///////
//////////////////////////////////
void programmingskills()
{
}

void isolation()
{
	auton = true;
	StartTask(arm_control);  //Run automatic intaking function
	drive(1,12,60,0);        //while driving to barrel
	StopTask(arm_control);
	regulate_arm(ARM_20);
	drive(1,24,127,0);
	regulate_intake(-1);
}

void interaction()
{
  regulate_arm(ARM_30);
  drive(1,48,127,0);       //Drive forward to 30"
  regulate_intake(-1);
}

//////////////////////////
/////// Autonomous ///////
//////////////////////////
task autonomous()
{
  if(SensorValue(JumperOne) == 0)
    programmingskills();
  else if(SensorValue(JumperTwo) == 0)
    isolation();
  else
    interaction();
}

//sign-finding function
float sign(float num)
{
  if(num==0)
    return 0;
  else
    return abs(num)/num;
}

/** Motor adjustment */
int motorValue(int joystick, int threshold, int splitPoint, int discreteMappings, int downSafe, int upSafe)
{
  float partition, result;
  float CONTROL_INTERVAL = (127 - splitPoint) / discreteMappings;
  float MOTOR_INTERVAL = (127 - splitPoint) / (discreteMappings - 1);
  int MOTOR_RANGE = splitPoint - threshold;
  if(joystick > downSafe && joystick < upSafe)
  {
    return 0;
  }
  //Continuous mapping from 0 at threshold to splitPoint at splitPoint
  else if(joystick < splitPoint && joystick > -splitPoint)
  {
    //TODO hacked to work!! - abs(downSafe) not bi-directionally compatible!
    return sign(joystick) * (threshold + MOTOR_RANGE * ((abs(joystick) - abs(downSafe)) / (splitPoint * 1.0))); // * sign(joystick);
  }
  else
  { //Discrete mappings: 64 to 80, 80 to 96, 96 to 112, 112 to 128
    result = splitPoint * sign(joystick);
    for(partition = splitPoint + 2 * CONTROL_INTERVAL; partition < 127; partition += CONTROL_INTERVAL)
    {
      if(joystick < partition && joystick > -partition)
      {
        result += MOTOR_INTERVAL * sign(joystick);
      }
    }
    return (int) result;
  }
}

//////////////////////////////
/////// Driver Control ///////
//////////////////////////////
task usercontrol()
{
  int lstick, lstick2, rstick, rstick2;
  auton = false;
  //Single-tasking
  StopTask(arm_control);
  StartTask(arm_control);
  while(true)
  {
    //DEBUG
    /*int joystickLeft1 = vexRT[Ch3];
    int joystickRight1 = vexRT[Ch2];
    int joystickLeft2 = vexRT[Ch3Xmtr2];
    int joystickRight2 = vexRT[Ch2Xmtr2];
    int potentiometer = SensorValue(pot);
    int leftShaftEncoder = SensorValue(lshaft);
    int rightShaftEncoder = SensorValue(rshaft);
    int ultrasonicSensor = SensorValue(ultra);
    int leftdrivebase = motor[leftDB];
    int rightdrivebase = motor[rightDB];*/
          //Input processing TODO filter
    lstick = vexRT[Ch3] * -1; //flip left drive base motor orientations
    rstick = vexRT[Ch2] * -1;
    lstick2 = vexRT[Ch3Xmtr2] * -1; //flip left lift motor orientations
    rstick2 = vexRT[Ch2Xmtr2];
    motor[port5] = motorValue(lstick, THRESHOLD, SPLIT_POINT, DISCRETE_MAPPINGS, BOTTOM_SAFE_L, TOP_SAFE_L);
    motor[port6] = motorValue(rstick, THRESHOLD, SPLIT_POINT, DISCRETE_MAPPINGS, BOTTOM_SAFE_R, TOP_SAFE_R);
    //Arm
    if(!arm_move || debug)
    {
      motor[port1] = lstick2 * -1;
      motor[port3] = lstick2;
      motor[port8] = lstick2 * -1;
      motor[port10] = lstick2;
    }
    //Intake
    if(!intake_move || debug)
      motor[port7] = rstick2;
  }
}